/*
 * This file contains all functions in the course Comp. Phys., including:
 ************************************************************************
 *   * job *			* algorithm *		* function *
 ************************************************************************
 * derivative of a 1-dim function:		 	Richardson_deriv
 * interpolation/extrapolation:				Nevill
 * numerical integration:	midpoint algorithm	integ_midpoint
 * 				trapzoid		integ_trapzoid
 * 				Simpson			Simpson
 * 				SelfAdaptive Simpson	SelfAdapt_Simpson
 * 				Gauss-Legendre		GauLeg
 * 							GauLegInt
 * nonlinear equation:		bisection algorithm	bisection
 * 				Newton method		Newton_findroot
 * 				secant method		SecantMethod
 * 1-dim optimization					findmin
 * equation set			Gaussian elimination	GaussLinear
 * 				LU decomposition	ludcmp
 * 							ludcmp_solve
 * 				tridiagonal case	TridiagLinear (not rebust yet)
 * diagonalization		jacobi algorithm	jacobi
 */

#define max_iteration 1E6

/*
 * Richardson算法：
 * 首先计算
 *      D^k_0 = (f(x+h/2^k) - f(x-h/2^k))/ ( h/2^{k-1} ), k=0,1,...,n
 * 然后根据递推公式
 *      D^k_m = D^{k+1}_{m-1} + \frac{ D^{k+1}_{m-1} - D^k_{m-1} }{ 4^m -1 }
 * 逐步推得　D^0_n 作为返回值，其误差为　O(h^{2(n+1)}
 *
 * 例：
 *
 * D^0_0
 * D^1_0        D^0_1
 * D^2_0        D^1_1   D^0_2
 *
 * 我们定义一个　3x3　的矩阵 deriv，然后逐列计算矩阵元，最后返回　D^0_2, 即 deriv[2][2] 的值。
 *
 * n 不能是负数，否则程序终止
 */
double Richardson_deriv(double x, double h, int n, double (*func)(double) ){

        double coef, y;

        double ** deriv = new double * [ n+1 ];//开辟动态内存，得到新的 (n+1)*(n+1) 矩阵
        for(int i=0;i<n+1;i++) deriv[i] = new double [n+1];

        for(int i=0;i<=n;i++){ // 赋值第一列 D^k_0, k=0,1,...,n
                coef = pow(0.5,i);// 1/(2^i)
                deriv[i][0] = ( func(x + h*coef ) - func(x - h*coef) ) / (2* h* coef);
        }
        for(int i=1;i<=n;i++){ // 赋值第　1,...,n 列
                for(int j=i;j<=n;j++){ // j 是行数，i是列数
                        deriv[j][i] = deriv[j][i-1] + (deriv[j][i-1] - deriv[j-1][i-1])/ (pow(4.0,i) -1 );//　这一行与递推公式对应
                }
        }
        //cout<<" error is O(h^"<<2*n+2<<")\n";
        y = deriv[n][n];
        for(int i=0;i<=n;i++)delete [] deriv[i];//释放动态内存
        delete [] deriv;

        return y;
}

/*
 * Nevill 算法
 *
 * 给定 n 个数据点　(x_data[], y_data[])，指定内插/外推点位置　x，Nevill()函数自动进行多项式拟合，得>到　x　处插值结果 y，并计算误差 dy
 *
 * 如果数据点的　x　坐标太靠近，误差可能会比较大，所以程序中断。
 *
 */
void Nevill(int n, double * x_data, double * y_data, double x, double &y, double & dy){

        double *c = new double [n];
        double *d = new double [n];

        int i, m, ns=0;
        double dis=fabs(x-x_data[0]), dif, den;
        for(int i=0;i<n;i++){
                if( dif=fabs(x-x_data[i]) < dis ){
			// 找到离x最近的数据点x[ns]，从它的函数值开始逐步推测y
                        dis = dif;
                        ns = i;
                }
                c[i] = y_data[i];//赋初值　c,d,　可以证明，这个初值带入公式，确然得到第二列　P_{01}, P_{12}, ...
                d[i] = y_data[i];
        }
        y = y_data[ns];//y的初始值

        for(m=1;m<n;m++){
                for(i=0;i<n-m;i++){
                        den = x_data[i] - x_data[i+m];
                        if( fabs(den) < 1E-9 ){
                                cout<<" two data points' x coordinates are too close.\n";
                                exit(1);
                        }
                        den = ( c[i+1] - d[i] )/ den;

                        d[i] = (x_data[i+m] - x) * den;//迭代
                        c[i] = (x_data[i] - x) * den;
                }
                y += ( dy = (2*ns < n-m ? c[ns]: d[--ns]) );// 这一行保证推测路径最接近直线。 ns < (n-m)/2，说明 ns 在上半区，由　c路径 （斜向下），否则由　d路径（斜向上）
        }
        delete [] c;
        delete [] d;
}
/*
 * integ_midpoint: 中值法积分
 * 给定区间[a,b]，分割为n份，被积函数为func，返回积分值
 */
double integ_midpoint(double a, double b, int n, double (*func)(double)){

        int i;
        double y=0;
        double h=(b-a)/n;
        for(i=0;i<n;i++){
                y += func( a + (i+0.5)*h );
        }
        y *= h;
        return y;
}
/*
 * integ_trapzoid: 梯形法积分
 * 给定区间[a,b]，分割为n份，被积函数为func，返回积分值
 */
double integ_trapzoid(double a, double b, int n, double (*func)(double)){

        int i;
        double y=func(a)/2;
        double h=(b-a)/n;
        for(i=1;i<=n-1;i++){
                y += func( a + i*h );
        }
        y += func(b)/2;
        y *= h;
        return y;
}

/*
 * Simpson：辛普生积分
 * 给定积分区间[a,b]，区间分割为n份，被积函数为func，返回积分值
 */
double Simpson(double a, double b, int n, double (*func)(double)){

        double y1,y2,y,h;
        int i;

        h = (b-a)/n;//步长

        y1 = 0;
        for(i=0;i<n;i++){
                y1 += func(a+ (i+0.5)*h);
        }
        y2 = 0;
        for(i=1;i<n;i++){
                y2 += func(a+ i*h);
        }
        y = 4*y1 + 2*y2 + func(a) + func(b);
        y *= h/6;
        return y;
}

/*
 * SelfAdapt_Simpson uses self adapted algorithm to calculate integration of func() over [a,b].
 * It automatically breaks [a,b] into unequal pieces, according to the local situaion of func().
 * the error is controled to be less than epsilon
 */
double SelfAdapt_Simpson(double a, double b, double (*func)(double), double epsilon){

        double y1 = Simpson(a, (a+b)/2, 1, func);
        double y2 = Simpson((a+b)/2, b, 1, func);
        double y3 = Simpson(a, b, 1, func);
        if( fabs(y1+y2-y3) < 15*epsilon ){
                return y3;
        }
        else{
                return SelfAdapt_Simpson(a, (a+b)/2, func, epsilon/2)
                        +
                        SelfAdapt_Simpson((a+b)/2, b, func, epsilon/2);
        }
}

/*
 * Given [a,b], returns abscissas x[] and weights w[], based on zeros of P_n(x)
 * Modified from Numerical Recipes
 */
void GauLeg(double a, double b, int n, double *x, double *w){

        double eps=1E-14;
        int m,j,i;
        double z1,z,xm,xl,pp,p3,p2,p1;

        m=(n+1)/2;
        xm=0.5*(a+b);
        xl=0.5*(b-a);
        for(i=0;i<m;i++){
                z=cos(M_PI*(i+0.75)/(n+0.5));//starting guess
                do{                     //Newton's method to get the ith zero
                        p1=1.0;         // P_0
                        p2=0.0;
                        for(j=0;j<n;j++){
                                p3=p2;  // P_{j-1}
                                p2=p1;  // P_{j}
                                p1= z*p2 + (z*p2-p3)*j/(j+1);
                                // P_{j+1},based on recurrence relation:
                                // (j+1)P_{j+1} = (2j+1)P_j - j P_{j-1}
                        }
                        pp=n*(z*p1-p2)/(z*z-1); // P'n(x_i), based on
                                    // (1-x^2)P'_n(x) = n P_{n-1}(x) - nxP_n (x)
                        z1=z;
                        z=z1-p1/pp; // Newton's method: s = x - f(x)/f'(x)
                }while(fabs(z-z1)>eps);
                x[i]=xm-xl*z;
                x[n-1-i]=xm+xl*z;
                w[i] = 2.0*xl/((1.0-z*z)*pp*pp); // formula of weights:
                                    // w[i] = 2/(1-x_j^2)/P'n(x_j)^2 for [-1,1]
                                    // multiplied by xl, for [a,b]
                w[n-1-i]=w[i];
        }
}

/*
 * Gauss-Legendre Integral
 * Order-n Gauss-Legendre integral, returns integral of func on [a,b]
 */
double GauLegInt(double a, double b, double (*func)(double), int n){

        if(n<1){
                cout<<"error: n<1 in GauLegInt(a,b,func,n).\n";
                exit(1);
        }
        int i;
        double y;

        double *x = new double [n];
        double *w = new double [n];

        GauLeg(a,b,n,x,w);//get the abscissas and weights

        y=0;
        for(i=0;i<n;i++){
                y += w[i] * func(x[i]);
        }

        delete [] x;
        delete [] w;

        return y;
}

/*
 * findroot: 在[a,b]区间内寻找 func(x)=0　的根，并返回根的值
 * 算法：二分法
 */
double bisection(double (*func)(double), double a, double b, double precision){

        double ya=func(a), yb=func(b);
        double c=(a+b)/2, yc;
        int i;

        if( ya* yb > 0){//如果 func(a) * func(b) >0, 则[a,b]区间内不一定有解，拒绝任务
                cout<<"error: invalid input [a,b] for findroot: f(a)*f(b)>0 "<<endl;
                exit(1);
        }

        for(i=0;i<max_iteration;i++){

                c = (a+b)/2;//区间中点
                yc = func(c);

                if( yc * ya <= 0 ){//如果 yc*ya<=0，说明[a,c]中有一个根
                        b=c;
                }
                else{//如果 yc*ya>0，说明[c,b]中有一个根
                        a=c;
                }
                if( fabs(a-b) < precision ){
			cout<<"\n iteration rounds = "<<i+1<<endl;
                        return (a+b)/2;
                }
        }
        cout<<"bisection method: failed to find a root, after "<<max_iteration<<" iterations.\n";
        exit(1);
}

/*
 * Newton's method: given a function (*func), which gives value and also the 1st derivative,
 * a suspicious zone [a,b], it finds a root in the zone within precision, and returns the root.
 */
double Newton_findroot(void (*func)(double, double &, double &), double a, double b, double precision){

        double x, dx, y, dy;
        int i;

        x = (a+b)/2;

        for(i=0;i<max_iteration;i++){
                func(x, y, dy);
                if( fabs(dy)<1E-9 ){
                        cout<<" error: f'=0 in Newton's root-finding method. \n";
                        exit(1);
                }
                dx = y/dy;
                x -= dx;
                if( (x-a)*(x-b) >0 ){
                        cout<<" error: x gets out of the given zone [a,b] in Newton's method.\n";
                        exit(1);
                }
                if( fabs(dx) < precision ){
			cout<<"\n iteration rounds = "<<i+1<<endl;
                        return x;
                }
        }
        cout<<" Newton's method: after "<<max_iteration<<" steps, failed to find a root.\n";
        exit(1);
}


/*
 * SecantMethod: given function func, suspicious zone [a,b], returns a root of func(x)==0
 */
double SecantMethod(double (*func)(double), double a, double b, double precision ){

        double c, fc, d, fd, e, fe;
        int i;

        c = (a+b)/2;//set starting points
        fc = func(c);
        d = (c+b)/2;
        fd = func(d);

        for(i=0;i<max_iteration;i++){
                e = d - (d-c)/(fd-fc)*fd;
                if( (e-a)*(e-b) >0 ){
                        cout<<"SecantMethod: error! iteration gets out of the given zone [a,b].\n";
                        exit(1);
                }
                fe = func(e);
                if( fabs(e-d)<precision && fabs(fe)<precision ){
			cout<<"\n iteration rounds = "<<i+1<<endl;
                        return e;
                }
                c=d;
                fc=fd;
                d=e;
                fd=fe;
        }

        cout<<"After "<<max_iteration<<" iterations, failed to find one root"<<endl;
        exit(1);
}

/*
 * StewensonMethod: given function func, suspicious zone [a,b], returns a root of func(x)==0
 */
double StewensonMethod(double (*func)(double), double a, double b, double precision ){

        double d, fd, e, fe;
        int i;

        d = (a+b)/2;
        fd = func(d);

        for(i=0;i<max_iteration;i++){
		if( fabs(fd)< 1E-15 ){
			cout<<"\n iteration rounds = "<<i+1<<endl;
			return d;
		}
                e = d - fd* fd/(fd - func(d - fd));
                if( (e-a)*(e-b) >0 ){
                        cout<<"StewensonMethod: error! iteration gets out of the given zone [a,b].\n";
                        exit(1);
                }
                fe = func(e);
                if( fabs(e-d)<precision ){
			cout<<"\n iteration rounds = "<<i+1<<endl;
                        return e;
                }
                d=e;
                fd=fe;
        }

        cout<<"After "<<max_iteration<<" iterations, failed to find one root"<<endl;
        exit(1);
}


/*
 * findmin: 给定函数func, 初始区间[a,b]，在精度要求precision下，寻找一个极小值点，并返回该点坐标
 */
double findmin(double (*func)(double), double a, double b, double precision){

        double temp,c,d;
        int i;

        if( func(a)<func(b) ){//寻找极小值的方向为：a->b->
                temp=a;
                a=b;
                b=temp;
        }

        for(i=0;i<max_iteration;i++){//寻找嫌疑区间
                c= b + 1.618*(b-a);
                if( func(c) > func(b) )break;
                a=b;
                b=c;
        }
        if(i==max_iteration){//10000次迭代以后，没有找到嫌疑区间，报错
                cout<<"after "<<max_iteration<<" iterations, didn't find a zone for fmin"<<endl;
                exit(1);
        }

        for(i=0;i<max_iteration;i++){
                if( fabs(a-c)<precision )break;
                d=0.38197*a + 0.61803*c;
                if( (d-a)*(d-b)<0){//如果d在a,b之间
                        if( func(d) < func(b) ){
                                c=b;
                                b=d;
                        }
                        else{
                                a=d;
                        }
                }
                else if( (d-a)*(d-b)>0 ){//如果d在b,c区间
                        if( func(d) < func(b) ){
                                a=b;
                                b=d;
                        }
                        else{
                                c=d;
                        }
                }
                //下面四行帮忙查错：观察每次迭代的结果
                //cout<<"-----------------------------------"<<endl;
                //cout<<"a="<<a<<", f(a)="<<func(a)<<endl;
                //cout<<"b="<<a<<", f(b)="<<func(b)<<endl;
                //cout<<"c="<<a<<", f(c)="<<func(c)<<endl;
        }
        return (a+c)/2;
}


/*
 * GaussLinear: solves n-dim linear equations
 * The solution is stored in x
 * Permutation of rows is activated, when the principle element is smaller than min
 * If A is singular, a permutation will fail and function exit(1), signals error
 */
void GaussLinear(int n, double **A, double *x, double min){

        int i,j,k;
        double y;
        for(i=0;i<n;i++){
                if( fabs( A[i][i] ) < min ){// permutation is activated
                        for(j=i+1;j<n;j++){
                                if( fabs( A[j][i] ) > min ){
                                        for(k=i;k<n;k++){
                                                y = A[i][k];
                                                A[i][k] = A[j][k];
                                                A[j][k] = y;
                                        }
                                        break;
                                }
                        }
                        if(j==n){// didn't find any element larger than min
                                cerr<<" A is singular in GaussLinear(n,A,x,min).\n";
                                exit(1);
                        }
                }
                for(j=i+1;j<n;j++){// row operations
                        y = - A[j][i] / A[i][i];
                        for(k=0;k<n+1;k++){
                                A[j][k] = y * A[i][k] + A[j][k];
                        }
                }
                // for test only
                /*
                cout<<"i="<<i<<"\t A:"<<endl;
                for(j=0;j<n;j++){
                        for(k=0;k<n+1;k++){
                                cout<<A[j][k]<<",";
                        }
                        cout<<endl;
                }
                */
        }
        for(j=n-1;j>=0;j--){
                y=0;
                for(k=j+1;k<n;k++) y += A[j][k]*x[k];
                x[j] = 1/A[j][j] * ( A[j][n] - y );
        }
}

/*
 * ludcmp():    对非奇异方阵A进行行变换，再进行LU分解
 * 基于Crout算法，将L矩阵的所有对角元都设为1，分解过后，L矩阵的非对角元+U矩阵所有元素，都写入输入的系数矩阵double **a，所以原来的系数矩阵被破坏。如果double **a是奇异阵，程序将会报错并退出。
 * int n:           维数
 * double **a:      n x n 的系数矩阵
 * int *pivot:      记录行变换，A经过这些行变换以后变成A'，A'的第i行，实则是A的第pivot[i]行
 * int sign:        如果做了偶数次行变换，sign值为1，否则为-1
 */
void ludcmp(int n, double **a, int *pivot, int & sign){

        int i,j,k,imax;
        double big, y;
        double *max = new double [n];//每行绝对值最大的元素值
        for(i=0;i<n;i++){
                big = 0;
                for(j=0;j<n;j++){
                        if( fabs(a[i][j]) > big ){
                                big = fabs( a[i][j] );
                        }
                }
                if( big < 1E-9 ){//如果有一行所有元素绝对值都小于1E-9，则认为double**a是奇异阵
                        cout<<"a is singular in ludcmp(n,a,pivot,sign).\n";
                        exit(1);
                }
                max[i] = big;
        }
        //cout<<"max:"; for(i=0;i<n;i++)cout<<max[i]<<","; cout<<endl;
        sign =1;
        for(j=0;j<n;j++){//依次对L、U的第0,...,n-1列进行计算
                for(i=0;i<=j;i++){
        // 计算U矩阵第j列，计算公式为 \beta_{ij} = a_{ij} - \sum^{i-1}_{k=0} \alpha_{ik} \beta_{kj}$
                        y=0;
                        for(k=0;k<= i-1;k++){
                                y+= a[i][k] * a[k][j];
                        }
                        a[i][j] -= y;
                }
                imax = j;
                big = fabs( a[j][j] ) / max[j]; // fabs( a_{jj}) / max_j, 即 a_{ij} 在第j行中的相对大小
                for(i=j+1;i<n;i++){ //
                        y=0;
                        for(k=0;k<=j-1;k++){
                                y+= a[i][k] * a[k][j];
                        }
                        a[i][j] -= y;
                        //计算L矩阵的第j行，计算公式为\alpha_{ij} = \frac{1}{\beta_{ij}}( a_{ij} - \sum^{i-1}_{k=0} \alpha_{ik} \beta_{kj} )，这里没有乘上\frac{1}{\beta_{jj}}$
                        if( y = fabs( a[i][j] ) / max[i] > big ){
                                big = y;
                                imax = i;
                        }
                        //在 \frac{a_{j,j}}{max[j]}, \frac{a_{j+1,j}}{max[j+1]},\cdots,\frac{a_{n-1,j}}{max[n-1]}中, \frac{a_{imax,j}}{max[imax]}最大，所以下面交换第imax与第j行。
                }
                if( imax != j ){// 交换第j行与第imax行
                        pivot[j] = imax;
                        sign *= -1;
                        y = max[j];// max[j]与max[imax]也需要交换
                        max[j] = max[imax];
                        max[imax] = y;
                        for(k=0;k<n;k++){
                                y = a[j][k];
                                a[j][k] = a[ imax ][k];
                                a[ imax ][k] = y;
                        }
                }
                else{
                        pivot[j] = j;
                }
                for(i=j+1;i<n;i++) a[i][j] /= a[j][j];//将L矩阵第j列的元素乘上\frac{1}{\beta_{jj}}$
                //for test only 每次操作完一列，可以输出来看看，用于检查代码
                /*
                cout<<"~~~~~~~~~ j="<<j<<endl;
                for(i=0;i<n;i++){
                        for(k=0;k<n;k++){
                                cout<<a[i][k]<<",";
                        }
                        cout<<endl;
                }
                */
        }
        delete [] max;
}
/*
 * ludcmp_solve:  解线性方程组 Ax=b, 结果 x 写入 b
 * a 不是系数矩阵 A, 而是A经过行变换后的A'的LU分解，是ludcmp()函数的结果，行变换记录在pivot中
 * 如果U是奇异阵，程序会报错并退出。
 * int n            矩阵维数
 * double ** a      A'的LU分解，A'是A经过行变换后的矩阵
 * int *pivot:      记录行变换，A经过这些行变换以后变成A'，A'的第i行，实则是A的第pivot[i]行
 */
void ludcmp_solve(int n, double ** a, int * pivot, double * b){

        int j,k;
        double y;
        // A经过行变换变为A'，对b进行同样的行变换，可以保证方程组的根不变
        for(j=0;j<n-1;j++){
                k = pivot[j];
                y = b[j];
                b[j] = b[k];
                b[k] = y;
        }
        //cout<<"换行操作之后，b'= "; for(j=0;j<n;j++)cout<<b[j]<<","; cout<<endl;
        // 经过行变换以后，方程组变为A'x=b'，而A'=LU，所以我们记y=Ux，则方程组变为Ly=b'，下面的代码迭代解出y，并将y的值写入double *b，覆盖原来的值
        for(j=0;j<n;j++){
                y=b[j];
                for(k=0;k<=j-1;k++){
                        y -= a[j][k] * b[k];
                }
                b[j] = y;
        }
        //cout<<"y:"; for(j=0;j<n;j++)cout<<b[j]<<","; cout<<endl;
        // 根据 y=Ux，迭代解出x，并将x的值写入double *b，覆盖原来的值
        for(j=n-1;j>=0;j--){
                y=b[j];
                for(k=j+1;k<n;k++){
                        y -= a[j][k] * b[k];
                }
                if( fabs(a[j][j])<1E-9 ){
                        cout<<"error: U is singular in the LU decomposition of A.\n";
                        exit(1);
                }
                b[j] = y/a[j][j];
        }
        //最终double *b中的值就是方程组的解
}

/*
 * TridiagLinear:       Solve linear equations Ax=r, where A has a tridiagonal form:
 *                      b0      c0
 *                      a1      b1      c1
 *                      0       a2      b2      c2
 *                      ...     ...     ...     ...
 *                      ...     ...     ...     ...
 *                                      a_{n-1} b_{n-1}
 *
 *                      The solution is stored in u
 */
void TridiagLinear( int n, double *a, double *b, double *c, double *u, double *r ){

        int j;
        double *gamma = new double [n];
        u[0] = r[0]/b[0];
        gamma[0] = 0;
        for(j=1;j<n;j++){
                gamma[j] = c[j-1]/(b[j-1] - a[j-1]*gamma[j-1]);
                u[j] = ( r[j] - a[j]*u[j-1] )/(b[j] - a[j]*gamma[j]);
        }
        for(j=n-2;j>=0;j--){
                u[j] -= gamma[j+1]*u[j+1];
        }
        delete [] gamma;
}

/* LeastSquare：最小二乘法拟合
 * int N            共有N组实验数据供拟合
 * double **x_data  x_data[i] 是第i组数据的各个实验条件构成的矢量\vec{x}_i$
 * double **y_data  y_data[i] 是第i组数据的y_i值
 * int n            每次实验有n个条件，即\vec{x}_i有n个分量
 * int m            理论公式有m个参数
 * double *a        理论公式参数为 a[0], ..., a[m-1]
 * double (**func)(int, double *)
 *                  理论公式为 f(x) = \sum^{m-1}_{i=0} a_i g_i(\vec{x}), func为指向函数的指针的指针，记录 g_0, ..., g_{m-1}$
 * double precision: GaussLinear需要的参数，在其中，如果主元小于precision，就触发换行操作
 */
double LeastSquare(int N, double **x_data, double *y_data, int n, int m, double *a, double (**func)(int, double *), double precision ){
        int l, i, j;
        double y,z;
        // double ** A_merge 是 A x = b 中系数矩阵A与b的并合矩阵，即 A_merge = (A|b)，请参考高斯消元课件，这里的 A_merge对应课件中的 \mathcal{A}$
        double ** A_merge = new double * [ m ];
        for(l=0;l<m;l++){
                A_merge[l] = new double [m+1];
                // A_merge的左半部分为A，课件中解释了，A_{li}=\sum^{N-1}_{j=0} g_l(\vec{x}_j)g_i(\vec{x}_j)$
                for(i=0;i<m;i++){
                        y=0;
                        for(j=0;j<N;j++){
                                y += func[l](n, x_data[j]) * func[i](n, x_data[j]);
                        }
                        A_merge[l][i] = y;
                }
                // A_merge的右半部分为b，课件中解释了，b_{l}=\sum^{N-1}_{j=0} g_l(\vec{x}_j)y_j$
                y=0;
                for(j=0;j<N;j++){
                        y += func[l](n, x_data[j]) * y_data[j];
                }
                A_merge[l][i] = y;
        }
        // for test only 在程序调试阶段，可以手写一个简单例子，并输出A_merge，看看程序算的与手推的是否一致
        /*
        cout<<"A_merge is ready"<<endl;
        for(i=0;i<m;i++){
                for(j=0;j<m+1;j++){
                        cout<<A_merge[i][j]<<",";
                }
                cout<<endl;
        }
        */
        GaussLinear(m, A_merge, a, precision);//求解Ax=b
        //下面的代码计算RMSD = \sqrt{ \frac{ \sum^{N-1}_{j=0} (f(\vec{x}_j)-y_j)^2 }{N} } = \sqrt{\Delta/N}，其中\Delta在课件中定义了。
        y=0;
        for(j=0;j<N;j++){
                z=0;
                for(i=0;i<m;i++)
                        z += a[i] * func[i](n, x_data[j]);
                z -= y_data[j];
                y += z*z;
        }
        y = sqrt( y/N );
        for(i=0;i<m;i++) delete [] A_merge[i];//释放动态内存
        delete [] A_merge;
        return y;// 返还RMSD
}

/*
 * jacobi(...): 用Jacobi算法对角化一个对称实矩阵，得到本征值和本征矢
 *
 * int n	矩阵维数
 * double *a	实对称阵A，a[i*n+j]表示A_{ij}，i,j=0,1,...,n-1
 * double *z	本征值：z[0],z[1],...,z[n-1]
 * 程序运行结束以后，本征矢量储存在 a 中，覆盖原来的输入矩阵，a的第i行是对应于z[i]的本征矢量，即a[ i*n +j ], j=0,...,n-1 是第i个本征矢量。
 */
void jacobi(int n, double *a, double *z)
{
	int l, i, j, p, q;
	double max, temp, s, c, theta, t;
	double g, h;
	double **v = new double *[n];//v矩阵初始时是一个nxn的单位阵，经过for(l)循环以后，v是所有相似矩阵的乘积，其列向量构成原来的矩阵 a 的特征向量。
	for(i=0;i<n;i++){
		v[i] = new double [n];
		for(j=0;j<n;j++){
			if(j!=i) v[i][j]=0;
			else v[i][j]=1;
		}
	}

	for (l = 0; l < max_iteration; l++)
	{
		max = 0;// 找到绝对值最大的非对角元,记下它的行数p、列数q
		for (i = 0; i < n; i++){
			for (j = 0; j < n; j++){
				if ( i != j && fabs( a[i*n+j] ) >max ){
					max = fabs( a[i*n+j] );
					p = i;
					q = j;
				}
			}
		}

		if ( max < 1e-10 ) break;
		//如果最大的非对角元已经小于1E-10，说明矩阵已经对角化了，所以跳出for(l)循环

		//构造相似变换
		theta = ( a[q*n+q] - a[p*n+p] )/(2*a[p*n+q]); //参考课件(70)式计算theta
		t = 1/ ( fabs(theta) + sqrt(theta*theta+1) );// 参考课件(75)式计算t
		if(theta<0) t *= -1;

		c = 1/sqrt(1+t*t); //参考课件(76)式计算c,s
		s = c*t;

		a[p*n+p] -= t* a[p*n+q];//参考课件(77-80)计算a'pp, a'qq, a'ip, a'iq
		a[q*n+q] += t* a[p*n+q];
		a[p*n+q] = 0;
		a[q*n+p] = 0;

		for(i=0;i<n;i++){//计算第p,q行，或第p,q列其它元素
			if(i!=p&&i!=q){
				temp = a[i*n+p];
				a[i*n+p] -= s*(a[i*n+q] + s/(1+c)*temp);
				a[i*n+q] += s*(temp - s/(1+c)*a[i*n+q]);
				a[p*n+i] = a[i*n+p];
				a[q*n+i] = a[i*n+q];
			}
		}
		for (i = 0; i < n; i++)// v = P1 P2 ... 
		{
			temp = v[i][p];
			v[i][p] =  temp * c - v[i][q] * s;
			v[i][q] =  temp * s + v[i][q] * c; 
		}
		/* for test only
		cout<<"a:"<<endl;
		for(i=0;i<n;i++){
			for(j=0;j<n;j++){
				cout<<a[i*n+j]<<",";
			}
			cout<<endl;
		}
		*/
	}//l
	if( l== max_iteration ){//如果经过for(l)循环以后，l的值为max_iteration，说明矩阵仍然没有对角化，我们就报告没有收敛，然后退出程序
		cout<<" Jacobi: after "<<max_iteration<<" iterations, didn't converge...\n";
		exit(1);
	}
	for (i = 0; i < n; i++)z[i] = a[i*n+i];//记录本征值到z中
	for(i=0;i<n;i++){ //把矩阵v中的数据录到a中，录完以后，a[i*n+0]~a[i*n+n-1]等于v的第i列
		for(j=0;j<n;j++){
			a[i*n+j] = v[j][i];
		}
	}
	for(i=0;i<n;i++) delete [] v[i]; delete [] v;//释放v的动态内存
}

/*
 * sort_eig(...):	将z中的特征值按从小到大排序，并对特征向量a做相应的变换
 * int n		维数
 * double *a		特征向量，a[i*n+0]~a[i*n+n-1]为第i个特征向量
 * double *z		特征值
 */
void sort_eig(int n, double *a, double *z){

	int i,j,l,p;
	double temp;

	for(p=0;p<n-1;p++){ // 排序：使得本征值按从小到大排列
	
		temp=z[p];
		l=p;
		for(i=p+1;i<n;i++){
			if(temp>z[i]){// 找到 z[p], ..., z[n-1] 中的最小值
				temp=z[i];
				l=i;
			}
		}
		if(l!=p)// 通过交换，把{z[p],...,z[n-1]}中的最小值放到z[p]的位置
		{
			temp=z[p];//交换 z[p] 与 z[l]
			z[p]=z[l];
			z[l]=temp;
			for(j=0;j<n;j++)// 交换 a[p][] 与 a[l][]
			{
				temp = a[p*n+j];
				a[p*n+j] = a[l*n+j];
				a[l*n+j] = temp;
			}
		}
	}                   
}

void jacobi_old(int n, double *a, double *z, double **v)
{
	int l, i, j, p, q;
	double max, temp, s, c, omega, x, y;
	double g, h;
	for (i = 0; i < n; i++)
	{
		for (j = 0; j < n; j++)
		{
			v[i][j] = 0;
		}
		v[i][i] = 1.0;         //v is a unit matrix
	}

	for (l = 1; l < max_iteration; l++)
	{
		max = 0;// find the largest matrix element
		for (i = 0; i < n; i++)
		{
			for (j = 0; j < n; j++)
			{
				if ( i != j && fabs( a[i*n+j] ) >max )
				{
					max = fabs( a[i*n+j] );
					p = i;
					q = j;
				}
			}
		}

		if ( max < 1e-10 ) 
			break;

		x = -a[p*n+q];
		y = ( a[q*n+q] - a[p*n+p] )/2.0; 
		omega = x / sqrt(x*x+y*y);
		if (y < 0) omega = -omega;
		s = 1.0 + sqrt(1.0 - omega*omega);
		s = omega / sqrt(2.0*s); 
		c = sqrt(1.0 - s*s);
		temp = a[p*n+p];
		a[p*n+p] = temp * c * c + a[q*n+q] *s *s + a[p*n+q] *omega;
		a[q*n+q] = temp * s * s + a[q*n+q] *c *c - a[p*n+q] *omega;
		a[p*n+q] = 0;
		a[q*n+p] = 0;

		for (j = 0; j < n; j++)
		{
			if (j != p && j != q)
			{
				temp = a[p*n+j]; 
				a[p*n+j] = temp * c + a[q*n+j] * s;
				a[q*n+j] = -temp * s + a[q*n+j] * c;
			}
		}
		for (i = 0; i < n; i++)
		{
			if (i != p && i != q) 
			{
				temp = a[i*n+p];
				a[i*n+p] =  temp * c + a[i*n+q] * s;
				a[i*n+q] = - temp * s + a[i*n+q] * c;
			}
		}
		for (i = 0; i < n; i++)
		{
			temp = v[i][p];
			v[i][p] =  temp * c + v[i][q] * s;
			v[i][q] = - temp * s + v[i][q] * c; 
		}
		/* for test only
		cout<<"a:"<<endl;
		for(i=0;i<n;i++){
			for(j=0;j<n;j++){
				cout<<a[i*n+j]<<",";
			}
			cout<<endl;
		}
		*/
	}//l
	if( l== max_iteration ){
		cout<<" Jacobi: after "<<max_iteration<<" iterations, didn't converge...\n";
		exit(1);
	}
	cout<<"HAHAHAHAHAH"<<endl;
	for (i = 0; i < n; i++)
		z[i] = a[i*n+i];
	for(p=0;p<n-1;p++)
	{
		temp=z[p];
		l=p;
		for(i=p+1;i<n;i++){// bubble sorting
			if(temp>z[i]){// find the smallest eigenvalue in z[p], ..., z[n-1]
				temp=z[i];
				l=i;
			}
		}
		cout<<"the smallest eigenvalue = "<< temp<<endl;
		if(l!=p)
		{
			temp=z[p];// put the smallest eigenvalue in {z[p],...,z[n-1]} in z[p]
			z[p]=z[l];
			z[l]=temp;
			for(j=0;j<n;j++)// exchange v[][p] with v[][l]
				// this can be optimized, we can store eigenwave in a row, rather than in a column
			{
				temp = v[j][p];
				v[j][p]=v[j][l];
				v[j][l]=temp;
			}
		}
	}                      //eigvalues from small to large
	for(i=0;i<n;i++)
	{
		temp=0;
		for(j=0;j<n;j++)
		{
			temp+=v[j][i]*v[j][i];
		}
		for(j=0;j<n;j++)
		{
			v[j][i] /= sqrt(temp);  //normalization of wavefunctions
		}
	}
}

/*
 * tqls: diagonalize a tridiagonal matrix in QL algorithm with shifting
 *
 * int n:		dimension
 * double *d		diagonal elements of the tridiagonal matrix, end up to be the eigen values
 * double *e		subdiagonal elements of the tridiagonal matrix
 * double **v		input as [n][n] matrix, whatever value it has, end up to be the eigen kets
 *
 * convention: e[0]=0
 *
 * d[0]	e[1]
 * e[1]	d[1]
 * ...
 * 			e[n-1]
 * 	  	e[n-1]	d[n-1]
 */
void tqls(int n, double *d, double *e, double **v){

	int m,l,i,iter,j;
	double c,s,r,theta,ks,app,aqq,apq,butterfly;

	// set v as elementary matrix
	for(i=0;i<n;i++){
		for(j=0;j<n;j++){
			if(i==j) v[i][j]=1;
			else v[i][j]=0;
		}
	}

	for(l=0;l<n-1;l++){
		iter=0;//迭代使得e[l+1]足够小，需要的步数
		do{
			for(m=l;m<n-1;m++)//e[m+1]是否已经足够小
				if( abs(e[m+1]) <= 1E-9*(abs(d[m])+abs(d[m+1]))) 
					break;
			//执行完上面的代码块，若e[l+1]足够小，则m=l；若e[l+p]足够小，则m=l+p-1;若都没有足够小，则m=n-1
			//下面的代码处理 l, ..., m 子矩阵块
			if(m!=l){//e[l+1]还没有足够小
				if(iter++ == 30){
					cout<<"Too many iterations in tqli, when trying to decrease e["<<l<<"] l=0,...,n-1 \n";
				}
				theta = (d[l+1]-d[l])/(2.0*e[l+1]);
				ks = - e[l+1]/(fabs(theta) + sqrt(theta*theta+1));
				if( theta < 0 ) ks *= -1;
				ks += d[l];// ks is set up, shifting
				
				for(i=l;i<=m;i++) d[i] -= ks;

				app = d[m-1]; aqq = d[m]; apq = e[m];
				r = sqrt( apq * apq + aqq * aqq);// c * apq - s *aqq = 0
				c = aqq / r;
				s = apq / r;
				// these codes heavily rely on formulas about Jacobi transformation on my Lecture notes, or on Numerical Recipes 
				d[m-1] = c*c* app - 2*c*s*apq + s*s*aqq;
				d[m] = s*s*app + 2*c*s*apq + c*c*aqq;
				e[m] = (c*c-s*s)*apq + c*s*(app-aqq);
				butterfly = s* e[m-1];// a'_{m-2,m} = a'_{m,m-2} = -s a_{m,m-1}
				e[m-1] = c* e[m-1];// a'_{m-2,m-1} = c*a_{m-2,m-1}

				for(j=0;j<n;j++){
					app = v[j][m-1];
					aqq = v[j][m];
					v[j][m-1] = c*app - s*aqq;
					v[j][m] = s*app + c*aqq;
				}

				for(i=m-1;i>=l+1;i--){
					r = sqrt( butterfly*butterfly + e[i+1]*e[i+1]);
					s = butterfly / r;
					c = e[i+1] / r;
					//butterfly = a_{i+1,i-1}, 
					//butterfly' = a'_{i+1,i-1} = c butterfly - s a_{i+1,i} = c butterfly - s e[i+1] = 0

					app = d[i-1]; aqq=d[i]; apq = e[i];
					d[i-1] = c*c* app - 2*c*s*apq + s*s*aqq;
					d[i] = s*s*app + 2*c*s*apq + c*c*aqq;
					e[i] = (c*c-s*s)*apq + c*s*(app-aqq);
					e[i+1] = s*butterfly + c*e[i+1];
					butterfly = s*e[i-1];
					e[i-1] = c*e[i-1];

					for(j=0;j<n;j++){
						app = v[j][i-1];
						aqq = v[j][i];
						v[j][i-1] = c*app - s*aqq;
						v[j][i] = s*app + c*aqq;
					}

				}
				for(i=l;i<=m;i++) d[i] += ks;
			}
		}while( m!=l );
	}
}
